/*
 * Copyright (c) 2020, Robobrain.
 * All rights reserved.
 *
 * Redistribution and use in source and binary forms, with or without
 * modification, are permitted provided that the following conditions are met:
 *
 *     * Redistributions of source code must retain the above copyright
 *       notice, this list of conditions and the following disclaimer.
 *     * Redistributions in binary form must reproduce the above copyright
 *       notice, this list of conditions and the following disclaimer in the
 *       documentation and/or other materials provided with the distribution.
 *     * Neither the name of the Willow Garage, Inc. nor the names of its
 *       contributors may be used to endorse or promote products derived from
 *       this software without specific prior written permission.
 *
 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
 * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
 * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
 * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
 * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
 * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
 * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
 * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
 * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
 * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
 * POSSIBILITY OF SUCH DAMAGE.
 */

/* Author: Konstantinos Konstantinidis */

#include "n_lidar_obj/l_shape_track/l_shape_tracker.hpp"

static inline double normalize_angle_positive(double angle)
{
  //Normalizes the angle to be 0 to 2*M_PI.
  //It takes and returns radians.
  return fmod(fmod(angle, 2.0 * M_PI) + 2.0 * M_PI, 2.0 * M_PI);
}
static inline double normalize_angle(double angle)
{
  //Normalizes the angle to be -M_PI circle to +M_PI circle
  //It takes and returns radians.
  double a = normalize_angle_positive(angle); // Normalizes the angle to be 0 to 2*M_PI
  if (a > M_PI)
    a -= 2.0 * M_PI;
  return a;
}

static inline double shortest_angular_distance(double from, double to)
{
  //Given 2 angles, this returns the shortest angular difference.
  //The inputs and outputs are radians.
  //The result would always be -pi <= result <= pi.
  //Adding the result to "from" results to "to".
  return normalize_angle(to - from);
}

LshapeTracker::LshapeTracker() {} //Creates a blank estimator

LshapeTracker::LshapeTracker(const double &x_corner, const double &y_corner, const double &L1, const double &L2, const double &theta, const double &dt)
{

  // Initialization of Dynamic Kalman Filter
  int n = 6;        // Number of states (x,y,vx,vy,ax,ay)
  int m = 2;        // Number of measurements (x_corner,y_corner)
  MatrixXd A(n, n); // System dynamics matrix
  MatrixXd C(m, n); // Output matrix
  MatrixXd Q(n, n); // Process noise covariance
  MatrixXd R(m, m); // Measurement noise covariance
  MatrixXd P(n, n); // Estimate error covariance

  //A << 1, 0,dt, 0,
  //0, 1, 0,dt,
  //0, 0, 1, 0,
  //0, 0, 0, 1;
  //
  double ddt = dt * dt / 2;
  A << 1, 0, dt, 0, ddt, 0,
      0, 1, 0, dt, 0, ddt,
      0, 0, 1, 0, dt, 0,
      0, 0, 0, 1, 0, dt,
      0, 0, 0, 0, 1, 0,
      0, 0, 0, 0, 0, 1;

  C << 1, 0, 0, 0, 0, 0,
      0, 1, 0, 0, 0, 0;

  Q << 1, 0, 0, 0, 0, 0,
      0, 1, 0, 0, 0, 0,
      0, 0, 10, 0, 0, 0,
      0, 0, 0, 10, 0, 0,
      0, 0, 0, 0, 20, 0,
      0, 0, 0, 0, 0, 20;
  R.setIdentity();
  R *= 10;
  R *= 0.1;
  P.setIdentity() * 0.1;

  KalmanFilter dynamic_kalman_filter(dt, A, C, Q, R, P);
  this->dynamic_kf = dynamic_kalman_filter;

  VectorXd x0_dynamic(n);
  x0_dynamic << x_corner, y_corner, 0, 0, 0, 0; // 初始化x状态向量没有速度，因为没有radar
  dynamic_kf.init(0, x0_dynamic);

  // Initialization of Shape Kalman Filter
  n = 4;             // Number of states: L1,L2,Theta,wmiga
  m = 3;             // Number of measurements
  MatrixXd As(n, n); // System dynamics matrix
  MatrixXd Cs(m, n); // Output matrix
  MatrixXd Qs(n, n); // Process noise covariance
  MatrixXd Rs(m, m); // Measurement noise covariance
  MatrixXd Ps(n, n); // Estimate error covariance

  As << 1, 0, 0, 0,
      0, 1, 0, 0,
      0, 0, 1, dt,
      0, 0, 0, 1;

  Cs << 1, 0, 0, 0,
      0, 1, 0, 0,
      0, 0, 1, 0;

  // Qs中theta wmiga 与dt有关, Rs初始化为0
  Qs << 0.04, 0, 0, 0,
      0, 0.04, 0, 0,
      0, 0, dt, pow(dt, 2) / 2,
      0, 0, 0, dt;
  Ps.setIdentity();

  KalmanFilter shape_kalman_filter(dt, As, Cs, Qs, Rs, Ps);
  this->shape_kf = shape_kalman_filter;

  VectorXd x0_shape(n);
  double L1init = 0.1;
  double L2init = 0.1;
  if (L1 > L1init)
  {
    L1init = L1;
  }
  if (L2 > L2init)
  {
    L2init = L2;
  }
  x0_shape << L1init, L2init, theta, 0;
  shape_kf.init(0, x0_shape);

  L1_old = L1;
  L2_old = L2;
  old_thetaL1 = theta;
  x_old = x_corner;
  y_old = y_corner;
}

void LshapeTracker::update(const double &thetaL1, const double &x_corner, const double &y_corner, const double &L1, const double &L2, const double &dt, const int cluster_size)
{

  current_size = cluster_size;
  // thetaL1为航向角，为当前帧的测量，(L1 L2 thetaL1), CornerPointSwitch在这里进行，赋值给：dynamic_kf/shape_kf.x_hat
  // 如果角度变化不大的话即无CornerPointSwitch：跳过这个函数
  detectCornerPointSwitch(old_thetaL1, thetaL1, dt); // 这里进行的是角点变化后的x of dynamic and shape state

  // 状态矩阵的thetaL1在detectCornerPointSwitch()函数已经进行了更改
  double norm = normalize_angle(shape_kf.state()(2)); // Normalizes the angle to be -M_PI circle to +M_PI circle
  double distance = shortest_angular_distance(norm, thetaL1);
  // 状态向量已经更新
  double theta = distance + shape_kf.state()(2); // 原值+theta角度变化值，才是目前的测量theta值

  // Update Dynamic Kalman Filter
  // y为卡尔曼的测量输入进去进行卡尔曼的更新阶段
  Vector2d y;
  y << x_corner, y_corner;
  // dynamic_kf.predict/update在这里更新,基于前面角点变化的新的状态：dynamic_kf/shape_kf.x
  // 预测用的是ca模型，更新的测量是l-shape算法直接输出的[thetaL1,x_corner,y_corner]
  dynamic_kf.update(y, dt);

  // Update Shape Kalman Filter
  Vector3d shape_measurements;
  double L1max, L2max;
  L2max = L2;
  L1max = L1;
  // shape state的R直接这样给,初始化的时候没有赋值
  shape_kf.R << pow(L1, -2), 0, 0,
      0, pow(L2, -2), 0,
      0, 0, 0.5;
  shape_measurements << L1max, L2max, theta;
  shape_kf.update(shape_measurements, dt);

  L1_old = L1;
  L2_old = L2;
  old_thetaL1 = thetaL1;
  x_old = x_corner;
  y_old = y_corner;
}

void LshapeTracker::detectCornerPointSwitchMahalanobis(const double &from, const double &to, const double L1, const double L2, const double x_corner, const double y_corner)
{
  // The purpose of this function is to detect potential corner point switches.
  // For this purpose it calculates the Mahalanobis distance between the previous
  // and the current measurements and based on the lowest distance it decides if
  // the corner point changed or not.

  double x_new = x_corner;
  double y_new = y_corner;
  double theta_new = to;
  double theta_corner = from;
  double x_c = dynamic_kf.state()(0);
  double y_c = dynamic_kf.state()(1);
  double L1_box = shape_kf.state()(0);
  double L2_box = shape_kf.state()(1);

  double x_corner_L1 = x_c + L1_box * cos(theta_corner);
  double y_corner_L1 = y_c + L1_box * sin(theta_corner);
  double theta_corner_L1 = normalize_angle(theta_corner + pi / 2);

  double x_corner_L2 = x_c + L2_box * sin(theta_corner);
  double y_corner_L2 = y_c - L2_box * cos(theta_corner);
  double theta_corner_L2 = normalize_angle(theta_corner + pi / 2);
  ROS_DEBUG_STREAM("simple: " << theta_corner - theta_new << ", findTurn: " << findTurn(theta_new, theta_corner));

  Eigen::Matrix<double, 5, 5> C;
  C.setZero();
  C(0, 0) = dynamic_kf.P(0, 0);
  C(0, 1) = dynamic_kf.P(0, 1);
  C(1, 0) = dynamic_kf.P(1, 0);
  C(1, 1) = dynamic_kf.P(1, 1);
  C(2, 2) = shape_kf.P(0, 0);
  C(2, 3) = shape_kf.P(0, 1);
  C(3, 2) = shape_kf.P(1, 0);
  C(3, 3) = shape_kf.P(1, 1);
  C(4, 4) = shape_kf.P(2, 2);

  Eigen::Matrix<double, 5, 1> means;
  means(0) = x_c - x_new;
  means(1) = y_c - y_new;
  means(2) = L1 - L1_box;
  means(3) = L2 - L2_box;
  means(4) = findTurn(theta_new, theta_corner);
  std::vector<double> mdistances;
  mdistances.push_back(means.transpose() * C.inverse() * means);

  means(0) = x_corner_L1 - x_new;
  means(1) = y_corner_L1 - y_new;
  means(2) = L1 - L2_box;
  means(3) = L2 - L1_box;
  means(4) = findTurn(theta_new, theta_corner_L1);
  mdistances.push_back(means.transpose() * C.inverse() * means);

  means(0) = x_corner_L2 - x_new;
  means(1) = y_corner_L2 - y_new;
  means(2) = L1 - L2_box;
  means(3) = L2 - L1_box;
  means(4) = findTurn(theta_new, theta_corner_L2);
  mdistances.push_back(means.transpose() * C.inverse() * means);

  int minElementIndex = std::min_element(mdistances.begin(), mdistances.end()) - mdistances.begin();

  if (minElementIndex == 2 && abs(mdistances[0] - mdistances[2]) > 0.1 && current_size > 1)
  {
    this->CounterClockwisePointSwitch();
  }
  else if (minElementIndex == 1 && abs(mdistances[0] - mdistances[1]) > 0.1 && current_size > 1)
  {
    this->ClockwisePointSwitch();
  }

  std::vector<double> distances;
  double euclidean;
  euclidean = sqrt(pow(x_c - x_new, 2) + pow(y_c - y_new, 2) + pow(findTurn(theta_new, theta_corner), 2) + pow(L1 - L1_box, 2) + pow(L2 - L2_box, 2));
  distances.push_back(euclidean);
  euclidean = sqrt(pow(x_corner_L1 - x_new, 2) + pow(y_corner_L1 - y_new, 2) + pow(findTurn(theta_new, theta_corner_L1), 2) + pow(L1 - L2_box, 2) + pow(L2 - L1_box, 2));
  distances.push_back(euclidean);
  euclidean = sqrt(pow(x_corner_L2 - x_new, 2) + pow(y_corner_L2 - y_new, 2) + pow(findTurn(theta_new, theta_corner_L2), 2) + pow(L1 - L2_box, 2) + pow(L2 - L1_box, 2));
  distances.push_back(euclidean);
}

void LshapeTracker::BoxModel(double &x, double &y, double &vx, double &vy, double &theta, double &psi, double &omega, double &L1, double &L2, double &length, double &width)
{
  L1 = shape_kf.state()(0);
  L2 = shape_kf.state()(1);
  theta = shape_kf.state()(2);
  //Equations 30 of "L-Shape Model Switching-Based precise motion tracking of moving vehicles"
  double ex = (L1 * cos(theta) + L2 * sin(theta)) / 2;
  double ey = (L1 * sin(theta) - L2 * cos(theta)) / 2;

  omega = shape_kf.state()(3);
  // x,y stands for the box center
  x = dynamic_kf.state()(0) + ex;
  y = dynamic_kf.state()(1) + ey;

  //Equations 31 of "L-Shape Model Switching-Based precise motion tracking of moving vehicles"
  //TODO test the complete equation also
  vx = dynamic_kf.state()(2);
  vy = dynamic_kf.state()(3);

  findOrientation(psi, length, width);
}

double LshapeTracker::findTurn(const double &new_angle, const double &old_angle)
{
  //https://math.stackexchange.com/questions/1366869/calculating-rotation-direction-between-two-angles
  double theta_pro = new_angle - old_angle;
  double turn = 0;
  if (-M_PI <= theta_pro && theta_pro <= M_PI)
  {
    turn = theta_pro;
  }
  else if (theta_pro > M_PI)
  {
    turn = theta_pro - 2 * M_PI;
  }
  else if (theta_pro < -M_PI)
  {
    turn = theta_pro + 2 * M_PI;
  }
  return turn;
}

// detectCornerPointSwitch
void LshapeTracker::detectCornerPointSwitch(const double &from, const double &to, const double dt)
{
  //Corner Point Switch Detection
  // delta turn=old_theta_L1-new_theta_L1: [-pi,pi]
  // 里面的网址显示：转换的turn在[-180,180]的区间，负数表示顺时针旋转turn right:from current to target，正数表示逆时针：turn left
  // 但是由于本代码计算: turn = old_theta_L1-new_theta_L1而不是 turn = target - current
  double turn = findTurn(from, to);
  // 弧度制，M_PI=3.14, -M_PI/4=-0.78, turn < -0.8意味着 turn<-45度

  if (turn < -0.8)
  {
    // CounterClockwise:逆时针: turn left from current to target
    this->CounterClockwisePointSwitch();
  }
  else if (turn > 0.6)
  {
    // 顺时针转的角度>34.3: turn right from current to current
    this->ClockwisePointSwitch();
  }
}

void LshapeTracker::findOrientation(double &psi, double &length, double &width)
{
  //This function finds the orientation of a moving object, when given an L-shape orientation

  std::vector<double> angles;
  double angle_norm = normalize_angle(shape_kf.state()(2));
  angles.push_back(angle_norm);
  angles.push_back(angle_norm + pi);
  angles.push_back(angle_norm + pi / 2);
  angles.push_back(angle_norm + 3 * pi / 2);

  double vsp = atan2(dynamic_kf.state()(3), dynamic_kf.state()(2)); // atan(vy,vx)
  double min = 1.56;
  double distance;
  double orientation;
  int pos;
  for (unsigned int i = 0; i < 4; ++i)
  {
    distance = abs(shortest_angular_distance(vsp, angles[i]));
    if (distance < min)
    {
      min = distance;
      orientation = normalize_angle(angles[i]);
      pos = i;
    }
  }
  if (pos == 0 || pos == 1)
  {
    length = shape_kf.state()(0);
    width = shape_kf.state()(1);
  }
  else
  {
    length = shape_kf.state()(1);
    width = shape_kf.state()(0);
  }

  psi = normalize_angle(orientation); // -M_PI, M_PI
}

void LshapeTracker::ClockwisePointSwitch()
{
  // Equation 17

  Vector6d new_dynamic_states = dynamic_kf.state(); // dynamic_kf:x_hat
  Vector4d new_shape_states = shape_kf.state();     // shape_kf:x_hat

  double L1 = shape_kf.state()(0);
  double L2 = shape_kf.state()(1);

  //x = x + L1 * cos(theta);
  new_dynamic_states(0) += L1 * cos(shape_kf.state()(2));
  //y = y + L1 * sin(theta);
  new_dynamic_states(1) += L1 * sin(shape_kf.state()(2));
  //vx = vx - L1 * omega * sin(theta);
  new_dynamic_states(2) -= L1 * shape_kf.state()(3) * sin(shape_kf.state()(2));
  //vy = vy + L1 * omega * cos(theta);
  new_dynamic_states(3) += L1 * shape_kf.state()(3) * cos(shape_kf.state()(2));
  //ax = ax - L1 * omega^2 * cos(theta);
  new_dynamic_states(4) -= L1 * pow(shape_kf.state()(3), 2) * cos(shape_kf.state()(2));
  //ay = ay - L1 * omega^2 * sin(theta);
  new_dynamic_states(5) -= L1 * pow(shape_kf.state()(3), 2) * sin(shape_kf.state()(2));

  //L1 = L2
  new_shape_states(0) = L2;
  //L2 = L1
  new_shape_states(1) = L1;

  new_shape_states(2) = shape_kf.state()(2) - pi / 2;

  dynamic_kf.changeStates(new_dynamic_states);
  shape_kf.changeStates(new_shape_states);
}

void LshapeTracker::CounterClockwisePointSwitch()
{
  // Equation 17

  Vector6d new_dynamic_states = dynamic_kf.state();
  Vector4d new_shape_states = shape_kf.state();

  double L1 = shape_kf.state()(0);
  double L2 = shape_kf.state()(1);

  //x = x + L2 * sin(theta);
  new_dynamic_states(0) += L2 * sin(shape_kf.state()(2));
  //y = y - L2 * cos(theta);
  new_dynamic_states(1) -= L2 * cos(shape_kf.state()(2));
  //vx = vx + L2 * omega * cos(theta);
  new_dynamic_states(2) += L2 * shape_kf.state()(3) * cos(shape_kf.state()(2));
  //vy = vy + L2 * omega * sin(theta);
  new_dynamic_states(3) += L2 * shape_kf.state()(3) * sin(shape_kf.state()(2));
  //ax = ax - L2 * omega^2 * cos(theta);
  new_dynamic_states(4) = dynamic_kf.state()(4) - L2 * pow(shape_kf.state()(3), 2) * sin(shape_kf.state()(2));
  //ay = ay - L2 * omega^2 * sin(theta);
  new_dynamic_states(5) = dynamic_kf.state()(5) + L2 * pow(shape_kf.state()(3), 2) * cos(shape_kf.state()(2));

  //L1 = L2
  new_shape_states(0) = L2;
  //L2 = L1
  new_shape_states(1) = L1;

  // 为啥theta加90度？？？
  new_shape_states(2) = shape_kf.state()(2) + pi / 2;

  dynamic_kf.changeStates(new_dynamic_states);
  shape_kf.changeStates(new_shape_states);
}
